package ncit.bluetooth.robots;

import ncit.bluetooth.connect.BluetoothChatService;
import android.os.Handler;
import android.util.Log;

public class NXTLego extends Robot{
	
	boolean goForward;
	float direction = 0;
	private static String TAG = "bluetooth.settings.NXTLego";
	
	
	public NXTLego()
	{
		
	}
	
	public NXTLego(Handler h){
		
		_controlHandler = h;
		
	}
	
	
	@Override
	public void startMotors(){
		byte power = (Commands.MAX_POWER_MOTOR);
		byte turnRatio = 0;

		
		Log.i(TAG,"a intrat in functia start motors.");

		setOutputMode(Commands.LEFT_BACK_MOTOR, power, Commands.SOS_MODE_MOTORON, Commands.SOS_REGMODE_MOTOR_SYNC,
				turnRatio, Commands.SOS_MOTOR_RS_RAMPUP, 0);
		
		setOutputMode(Commands.RIGHT_BACK_MOTOR, power, Commands.SOS_MODE_MOTORON, Commands.SOS_REGMODE_MOTOR_SYNC,
				turnRatio, Commands.SOS_MOTOR_RS_RAMPUP, 0);
		
		
	}
	
	@Override
	public void goForwardBackward(float slope){
		byte power=0;
		float semn=1;
		
		semn=Math.abs(slope)/slope;
		
		if(slope<-0.2||slope>0.2)
			power = (byte)(semn * ( 25 + Commands.MAX_POWER_MOTOR * ( ( semn*slope-0.2) / 0.8 )));
		else motorBreak();
		
		Log.i(TAG,"a intrat in functia go forward backwards power = " + (int)power);

		int turationLimit = Commands.TURATION_LIMIT;
		
		setOutputMode(Commands.LEFT_BACK_MOTOR, power, Commands.SOS_MODE_MOTORON, Commands.SOS_REGMODE_MOTOR_SYNC,
				(byte) 0, Commands.SOS_MOTOR_RS_RUNNING, turationLimit);
		
		setOutputMode(Commands.RIGHT_BACK_MOTOR, power, Commands.SOS_MODE_MOTORON, Commands.SOS_REGMODE_MOTOR_SYNC,
				(byte)0, Commands.SOS_MOTOR_RS_RUNNING, turationLimit);

	}
	
	@Override
	public void motorBreak(){
		byte power = 0;
		byte turnRatio = 0;
		
		setOutputMode(Commands.LEFT_BACK_MOTOR, power, Commands.SOS_MODE_BREAKE, Commands.SOS_REGMODE_MOTOR_SYNC,
				turnRatio, Commands.SOS_MOTOR_RS_RAMPDOWN, 1);
		
		setOutputMode(Commands.RIGHT_BACK_MOTOR, power, Commands.SOS_MODE_BREAKE, Commands.SOS_REGMODE_MOTOR_SYNC,
				turnRatio, Commands.SOS_MOTOR_RS_RAMPDOWN, 1);
		
		setOutputMode(Commands.DIRECTION_MOTOR, power, Commands.SOS_MODE_BREAKE, Commands.SOS_REGMODE_MOTOR_SYNC,
				turnRatio, Commands.SOS_MOTOR_RS_RAMPDOWN, 1);
	}
	
	@Override
	public void goLeftRight(float slope){
		
		byte power=0;
		
		float semn=1;
		

		Log.d("slope","Slope-ul pentru left-right este: " + slope);
		
		float help = 0;

		help = slope - direction;

		if(help > 1){
			
			help = 1;
			direction = direction + help;
			
		}
		else if(help < -1){
			
			help = -1;
			direction = direction + help;
			
		}else direction = slope;

		slope = help;
		
		semn=Math.abs(slope)/slope;
		semn=(Math.abs(slope))/slope;

		
		if((int)(slope/0.1)!=0)
		 	power = (byte) (semn*40+6*(int)(slope/0.1));

		setOutputMode(Commands.DIRECTION_MOTOR, power, Commands.SOS_MODE_MOTORON, Commands.SOS_REGMODE_IDLE,
				(byte) 50, Commands.SOS_MOTOR_RS_RUNNING, 2);
	}
	
	@Override
	public void park(){
		
		Log.i(TAG, "Now parking..." );	
			
		try {
			Thread.sleep(1000);
			
			setOutputMode(Commands.DIRECTION_MOTOR, (byte) ((byte) -25 -Commands.MAX_POWER_MOTOR), Commands.SOS_MODE_MOTORON, Commands.SOS_REGMODE_IDLE,
					(byte) 0, Commands.SOS_MOTOR_RS_RUNNING, 1);
			Thread.sleep(1000);
		
			setOutputMode(Commands.LEFT_BACK_MOTOR, Commands.MAX_POWER_MOTOR, Commands.SOS_MODE_MOTORON, Commands.SOS_MODE_REGULATED,
				(byte) 0, Commands.SOS_MOTOR_RS_RUNNING, 0);
			
			setOutputMode(Commands.RIGHT_BACK_MOTOR, Commands.MAX_POWER_MOTOR, Commands.SOS_MODE_MOTORON, Commands.SOS_MODE_REGULATED,
				(byte) 0, Commands.SOS_MOTOR_RS_RUNNING, 0);
		
			Thread.sleep(8000);
			motorBreak();
			
			Thread.sleep(1000);
			setOutputMode(Commands.DIRECTION_MOTOR, (byte) (100), Commands.SOS_MODE_MOTORON, Commands.SOS_REGMODE_IDLE,
					(byte) 0, Commands.SOS_MOTOR_RS_RUNNING, 1);
			
			Thread.sleep(100);


			setOutputMode(Commands.LEFT_BACK_MOTOR, Commands.MAX_POWER_MOTOR, Commands.SOS_MODE_MOTORON, Commands.SOS_MODE_REGULATED,
					(byte) 0, Commands.SOS_MOTOR_RS_RUNNING, 0);

			setOutputMode(Commands.RIGHT_BACK_MOTOR, Commands.MAX_POWER_MOTOR, Commands.SOS_MODE_MOTORON, Commands.SOS_MODE_REGULATED,
					(byte) 0, Commands.SOS_MOTOR_RS_RUNNING, 0);
			
			Thread.sleep(500);
			
			motorBreak();

		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}
	
	public boolean play_sound(String filename){
		
		byte[] namebuffer = filename.getBytes();
		
		if(namebuffer.length > 15) {
			
			Log.d(TAG, "Sound file name is too long.");
			
			return false;
		}
		
		byte[] buffer = new byte[namebuffer.length + 3];
		buffer[0] = Commands.SEND_NO_REPLY;
		buffer[1] = 0x02;
		buffer[2] = 0x00;
		for(int i =0; i < namebuffer.length; i++)
			buffer[i+3] = namebuffer[i]; 
		
		
		//here we send to the device
		BluetoothChatService.getInstance().write(buffer);
		
		return true;
	}
	
	
	public boolean set_input_mode(byte input_port, byte sensor_type, byte sensor_mode)
	{
		byte[] buffer = new byte[5];
		byte port;
		
		switch(input_port)
		{
			case 0: port = 0x00; break;
			case 1: port = 0x01; break;
			case 2: port = 0x02; break;
			default: Log.d(TAG, "Invalid port number on set input mode");return false;
		}
		
		buffer[0] = Commands.SEND_NO_REPLY;
		buffer[1] = 0x05;
		buffer[2] = port; 
		buffer[3] = sensor_type;
		buffer[4] = sensor_mode;
		
		//here we send
		BluetoothChatService.getInstance().write(buffer);
		
		
		return true;
	}
	
	public boolean get_input_values(int input_port)
	{
		byte port;
		byte[] buffer = new byte[3];
		
		switch(input_port)
		{
		case 0: port = 0x00; break;
		case 1: port = 0x01; break;
		case 2: port = 0x02; break;
		default: Log.d(TAG, "Invalid port number on set input mode");return false;
		}
		
		buffer[0] = 0x00;
		buffer[1] = 0x07;
		buffer[2] = port;
		
		BluetoothChatService.getInstance().write(buffer);
		return true;
	}
	
	
	public void resetMotorPosition(byte motor, boolean relative){
		
		byte[] data = new byte[4];
		data[0] = 0x00;
		data[1] = 0x0A;
		data[2] = motor;
		data[3] = (byte) (relative?1:0); //true pt pozitia relativa de la ultima miscare
										 //false pt pozitia absoluta
		
		BluetoothChatService.getInstance().write(data);
		
	}
	
	private void resetInputValues(byte port){
		
		byte[] data = new byte [3];
		
		data[0] = (byte) 0x80;
		data[1] = 0x08;
		data[2] = port;
		
		BluetoothChatService.getInstance().write(data);
		
	}
	
	public void setOutputMode(byte motor, byte power, byte motorMode, byte motorRegulationMode,
							  byte turnRatio, byte motorRunState, int tachoLimit){
		byte[] data = new byte [13];
		
		data[0] = (byte) 0x80;
		data[1] = 0x04;
		data[2] = motor;
		data[3] = power; 
		data[ 4 ] = motorMode;
		data[ 5 ] = motorRegulationMode;
		data[ 6 ] = turnRatio;
		data[ 7 ] = motorRunState;
		data[ 8 ] = (byte) ( tachoLimit & 0xFF );   // Byte 8-12: tacho limit ULONG
		data[ 9 ] = (byte) ( tachoLimit >> 8 );
		data[ 10 ] = (byte) ( tachoLimit >> 16 );
		data[ 11 ] = (byte) ( tachoLimit >> 24 );
		
		
		Log.i(TAG," pragateste sa scriese in functia go forward backwards.");
		BluetoothChatService.getInstance().write(data);
	}
	
	public void getOutputState(byte motor){
		byte[] data = new byte[3];
		data[0] = 0x00;
		data[1] = 0x06;
		data[2] = motor;
		
		BluetoothChatService.getInstance().write(data);
		BluetoothChatService.getInstance().write(data);
	}

	@Override
	public void startSensors() {
		
		resetInputValues(Commands.ULTRASOUND_SENSOR);
		set_input_mode(Commands.ULTRASOUND_SENSOR, (byte)0x0B, (byte)0x80);
		
		resetInputValues(Commands.LEFT_BACK_TOUCH_SENSOR);
		set_input_mode(Commands.LEFT_BACK_TOUCH_SENSOR, (byte)0x01, (byte)0x20);
		
		resetInputValues(Commands.RIGHT_BACK_TOUCH_SENSOR);
		set_input_mode(Commands.RIGHT_BACK_TOUCH_SENSOR, (byte)0x01, (byte)0x20);
		
	}

	@Override
	public void askSensors() {
		
		if (goForward)
    		get_input_values(Commands.ULTRASOUND_SENSOR);
    	else{
    		
    		get_input_values(Commands.RIGHT_BACK_TOUCH_SENSOR);
    		get_input_values(Commands.LEFT_BACK_TOUCH_SENSOR);
    	}
		
		
	}
	
	
}

